from matplotlib import markers
import numpy as np
from cv2 import cv2
import re
import matplotlib.pyplot as plt
from utils2 import *
import random
import matplotlib
import random
import copy
import pickle
from sklearn import decomposition


round1_feature_path = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round2/supplementSampleByHumanNpyFiles/320pairWithRound1Supplement_moreDataAug_grayOpticalFlow/round1.npy'
round2_feature_path = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round2/supplementSampleByHumanNpyFiles/320pairWithRound1Supplement_moreDataAug_grayOpticalFlow/round2.npy'
round3_feature_path = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round2/supplementSampleByHumanNpyFiles/320pairWithRound1Supplement_moreDataAug_grayOpticalFlow/round3.npy'

round1_position_path = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round1/round1_sync_position.npy' # 经纬度
round2_position_path = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round2/round2_sync_position.npy' # 经纬度
round3_position_path = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round3/round3_sync_position.npy' # 经纬度
anchor_training_index_path = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round2/anchorImgs_method2_exp2/round2_method2_exp2_training_index.txt'
anchor_referenceLabel_path = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/round2/anchorReferenceLabel_method2_exp2.txt' # 默认没有reference label
pkuBirdViewImg = 'D:\\Research\\2020ContrastiveLearningForSceneLabel\\Data\\pkuBirdView.png'
index_map = 'E:/Research/2020ContrastiveLearningForSceneLabel/Data/20210329ExperimentData/IndexMap.pickle'


index_map = open(index_map,'rb')
index_map = pickle.load(index_map)
index_map = index_map[1]

anchor_pos_list, anchor_neg_list = getAnchorPosNegIdx4(anchor_training_index_path)
anchor_idx = [anchor_pos_list[i][0] for i in range(len(anchor_pos_list))]
training_idx = []
for i in anchor_pos_list:
    training_idx += i[1]

f = open(anchor_referenceLabel_path,'r')
lines = f.readlines()
f.close()
rf_label_anchor = [int(i[0]) for i in lines]

rf_label_training = [rf_label_anchor[i] for i in range(len(rf_label_anchor)) for j in range(16)]
rf_label_anchor = np.array(rf_label_anchor)
rf_label_training = np.array(rf_label_training)

print(np.array(anchor_idx)[rf_label_anchor == 3])

round1_feat = np.load(round1_feature_path)
round2_feat = np.load(round2_feature_path)
round3_feat = np.load(round3_feature_path)

rotate = 0 # 角度制
shiftX = 625
shiftY = 620
dx = 0.777
dy = 0.777 # 以上参数都是手调的
alpha = 0.5
GNSS_round2 = np.load(round2_position_path)
GNSS_round2[:,0] = -GNSS_round2[:,0]
GNSS_round2[:,1] *= dx
GNSS_round2[:,0] *= dy
GNSS_round2[:,1] += shiftX
GNSS_round2[:,0] += shiftY
GNSS_round1 = np.load(round1_position_path)
GNSS_round1[:,0] = -GNSS_round1[:,0]
GNSS_round1[:,1] *= dx
GNSS_round1[:,0] *= dy
GNSS_round1[:,1] += shiftX
GNSS_round1[:,0] += shiftY
GNSS_round3 = np.load(round3_position_path)
GNSS_round3[:,0] = -GNSS_round3[:,0]
GNSS_round3[:,1] *= dx
GNSS_round3[:,0] *= dy
GNSS_round3[:,1] += shiftX
GNSS_round3[:,0] += shiftY
cmap = 'rainbow'

# ==========================数据加载完毕==========================
sample_index = 18573
sample_feat = round2_feat[sample_index]
sample_feat = sample_feat[np.newaxis,:]

pca = decomposition.PCA(n_components=2)
pca.fit(round2_feat[training_idx])
round2_training_feat_pca = pca.transform(round2_feat[training_idx])
round2_sample_feat_pca = pca.transform(round2_feat[sample_index][None,:])

# ==================第二圈某点与第一圈相似度==================

sample_to_all_frame_sim = np.matmul(sample_feat, round1_feat.transpose(1,0))
sample_to_all_frame_sim = sample_to_all_frame_sim[0]
sample_to_all_frame_sim[0] = -1
sample_to_all_frame_sim[1] = 1

round1_mapping_index = index_map[sample_index][0]

mapping_feat_pca = pca.transform(round1_feat[round1_mapping_index][None,:])
fig = plt.figure()
ax = fig.add_subplot(111)
ax.scatter(round2_training_feat_pca[:,0], round2_training_feat_pca[:,1], c = rf_label_training, cmap = cmap, alpha=0.5)
ax.scatter(mapping_feat_pca[0,0], mapping_feat_pca[0,1])
ax.scatter(round2_sample_feat_pca[0,0], round2_sample_feat_pca[0,1],marker='+',c='black',zorder=3)
cbar = plt.colorbar(matplotlib.cm.ScalarMappable(cmap=cmap))
cbar.set_ticks([0,0.5,1])
cbar.set_ticklabels(['SR','TR','AT'])
plt.title('project round1 index %d to round2 feature'%round1_mapping_index)
plt.axis('equal')

# 卫星图
fig = plt.figure()
ax = fig.add_subplot(111)
img = gImage.imread(pkuBirdViewImg)
img = img[0:1087,:,:]
img[:,:,3] = alpha
ax.imshow(img, zorder = 0)

ax.scatter(GNSS_round1[:,1], GNSS_round1[:,0], s=5, c = sample_to_all_frame_sim, cmap=cmap, zorder = 1) # 画轨迹
ax.scatter(GNSS_round1[round1_mapping_index,1], GNSS_round1[round1_mapping_index,0], s=5, c = 'none', edgecolors='black') # 画轨迹
# ax.scatter(GNSS_round2[training_idx,1], GNSS_round2[training_idx,0], s=5, c='none',marker='o',edgecolors='black', alpha = 0.8) # 画所有训练数据
# for i in anchor_idx:
#     ax.text(GNSS_round2[i,1],GNSS_round2[i,0], str(i), fontsize=10, alpha = 0.5) # 画部分训练数据的索引

cbar = plt.colorbar(matplotlib.cm.ScalarMappable(cmap=cmap))
cbar.set_ticks([0,0.5,1])
cbar.set_ticklabels(['-1','0','1'])
plt.axis('equal')
ax.set_title('round2 index %d similarity to round 1'%sample_index)

plt.figure()
plt.scatter(range(len(sample_to_all_frame_sim)), sample_to_all_frame_sim, s=5)
plt.plot(sample_to_all_frame_sim)
plt.scatter(round1_mapping_index, sample_to_all_frame_sim[round1_mapping_index],c = 'r', zorder=3)
# plt.scatter(training_idx, sample_to_all_frame_sim[training_idx],c = 'black')
plt.title('round2 index %d similarity to round1, mapping index %d'%(sample_index, round1_mapping_index))

# ==================第二圈某点与第三圈相似度==================
sample_to_all_frame_sim = np.matmul(sample_feat, round3_feat.transpose(1,0))
sample_to_all_frame_sim = sample_to_all_frame_sim[0]
sample_to_all_frame_sim[0] = -1
sample_to_all_frame_sim[1] = 1

round3_mapping_index = index_map[sample_index][2]
round3_mapping_index = 10585


mapping_feat_pca = pca.transform(round3_feat[round3_mapping_index][None,:])
fig = plt.figure()
ax = fig.add_subplot(111)
ax.scatter(round2_training_feat_pca[:,0], round2_training_feat_pca[:,1], c = rf_label_training, cmap = cmap, alpha=0.5)
ax.scatter(mapping_feat_pca[0,0], mapping_feat_pca[0,1])
ax.scatter(round2_sample_feat_pca[0,0], round2_sample_feat_pca[0,1],marker='+',c='black',zorder=3)
cbar = plt.colorbar(matplotlib.cm.ScalarMappable(cmap=cmap))
cbar.set_ticks([0,0.5,1])
cbar.set_ticklabels(['SR','TR','AT'])
plt.title('project round3 index %d to round2 feature'%round3_mapping_index)
plt.axis('equal')

# 卫星图
fig = plt.figure()
ax = fig.add_subplot(111)
img = gImage.imread(pkuBirdViewImg)
img = img[0:1087,:,:]
img[:,:,3] = alpha
ax.imshow(img, zorder = 0)

ax.scatter(GNSS_round3[:,1], GNSS_round3[:,0], s=5, c = sample_to_all_frame_sim, cmap=cmap, zorder = 1) # 画轨迹
ax.scatter(GNSS_round3[round3_mapping_index,1], GNSS_round3[round3_mapping_index,0], s=5, c = 'none', edgecolors='black') # 画轨迹
# ax.scatter(GNSS_round2[training_idx,1], GNSS_round2[training_idx,0], s=5, c='none',marker='o',edgecolors='black', alpha = 0.8) # 画所有训练数据
# for i in anchor_idx:
#     ax.text(GNSS_round2[i,1],GNSS_round2[i,0], str(i), fontsize=10, alpha = 0.5) # 画部分训练数据的索引

cbar = plt.colorbar(matplotlib.cm.ScalarMappable(cmap=cmap))
cbar.set_ticks([0,0.5,1])
cbar.set_ticklabels(['-1','0','1'])
plt.axis('equal')
ax.set_title('round2 index %d similarity to round3'%sample_index)

plt.figure()
plt.scatter(range(len(sample_to_all_frame_sim)), sample_to_all_frame_sim, s=5)
plt.plot(sample_to_all_frame_sim)
plt.scatter(round3_mapping_index, sample_to_all_frame_sim[round3_mapping_index],c = 'r', zorder=3)
# plt.scatter(training_idx, sample_to_all_frame_sim[training_idx],c = 'black')
plt.title('round2 index %d similarity to round3, mapping index %d'%(sample_index, round3_mapping_index))

# ==================第二圈某点与第二圈相似度==================
sample_to_all_frame_sim = np.matmul(sample_feat, round2_feat.transpose(1,0))
sample_to_all_frame_sim = sample_to_all_frame_sim[0]
sample_to_all_frame_sim[0] = -1
sample_to_all_frame_sim[1] = 1

round2_mapping_index = sample_index
# round2_mapping_index = 3475

mapping_feat_pca = pca.transform(round2_feat[round2_mapping_index][None,:])
fig = plt.figure()
ax = fig.add_subplot(111)
ax.scatter(round2_training_feat_pca[:,0], round2_training_feat_pca[:,1], c = rf_label_training, cmap = cmap, alpha=0.5)
ax.scatter(mapping_feat_pca[0,0], mapping_feat_pca[0,1])
ax.scatter(round2_sample_feat_pca[0,0], round2_sample_feat_pca[0,1],marker='+',c='black',zorder=3)
cbar = plt.colorbar(matplotlib.cm.ScalarMappable(cmap=cmap))
cbar.set_ticks([0,0.5,1])
cbar.set_ticklabels(['SR','TR','AT'])
plt.title('project round2 index %d to round2 feature'%round2_mapping_index)
plt.axis('equal')

# 卫星图
fig = plt.figure()
ax = fig.add_subplot(111)
img = gImage.imread(pkuBirdViewImg)
img = img[0:1087,:,:]
img[:,:,3] = alpha
ax.imshow(img, zorder = 0)

ax.scatter(GNSS_round2[:,1], GNSS_round2[:,0], s=5, c = sample_to_all_frame_sim, cmap=cmap, zorder = 1) # 画轨迹
ax.scatter(GNSS_round2[round2_mapping_index,1], GNSS_round2[round2_mapping_index,0], s=5, c = 'none', edgecolors='black') # 画轨迹
# ax.scatter(GNSS_round2[training_idx,1], GNSS_round2[training_idx,0], s=5, c='none',marker='o',edgecolors='black', alpha = 0.8) # 画所有训练数据
# for i in anchor_idx:
#     ax.text(GNSS_round2[i,1],GNSS_round2[i,0], str(i), fontsize=10, alpha = 0.5) # 画部分训练数据的索引

cbar = plt.colorbar(matplotlib.cm.ScalarMappable(cmap=cmap))
cbar.set_ticks([0,0.5,1])
cbar.set_ticklabels(['-1','0','1'])
plt.axis('equal')
ax.set_title('round2 index %d similarity to round2'%sample_index)

plt.figure()
plt.scatter(range(len(sample_to_all_frame_sim)), sample_to_all_frame_sim, s=5)
plt.plot(sample_to_all_frame_sim)
plt.scatter(round2_mapping_index, sample_to_all_frame_sim[round2_mapping_index],c = 'r', zorder=3)
# plt.scatter(training_idx, sample_to_all_frame_sim[training_idx],c = 'black')
plt.title('round2 index %d similarity to round2, mapping index %d'%(sample_index, round2_mapping_index))

# ===========================================================================================.
print('round1 %d and round2 %d similarity: %.5f'%(round1_mapping_index, sample_index, np.sum(round1_feat[round1_mapping_index] * round2_feat[sample_index])))
print('round2 %d and round3 %d similarity: %.5f'%(sample_index, round3_mapping_index, np.sum(round2_feat[sample_index] * round3_feat[round3_mapping_index])))
print('round1 %d and round3 %d similarity: %.5f'%(round1_mapping_index, round3_mapping_index, np.sum(round1_feat[round1_mapping_index] * round3_feat[round3_mapping_index])))


plt.show()

pass